#ifndef PERPROCESS_H_
#define PERPROCESS_H_
#include "architecture/algorithm.h"
#include "architecture/preprocess/filter/conditional_filter.h"
#include "architecture/preprocess/filter/radius_filter.h"
#include "architecture/preprocess/filter/removeNan.h"
#include "architecture/preprocess/filter/voxel_filter.h"
#include "architecture/preprocess/lidar_preprocess.h"
// #include "architecture/preprocess/patchwork++/include/patchworkpp/patchworkpp.hpp"
#include "architecture/preprocess/seg/average_ground_seg.h"
#include "architecture/preprocess/seg/average_ground_seg.hpp"
#include "common/data_type.h"
#include "common/threadpool.h"
#include "multi_process_points/multi_process_points.h"
#include "multi_process_points/multi_process_points.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <iomanip>
#include <math.h>
#include <memory>
#include <mutex>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <unistd.h>
#define SET_NAME(front, back) front##back // 定义可变变量名
namespace perception
{
    namespace lidar_preprocess
    {
        class PreProcess
        {
        public:
            PreProcess(rclcpp::Node *private_nh) : private_nh_(private_nh)
            {
                preprocess1 = std::make_shared<perception::lidar_preprocess::RemoveNan<XYZI_1, XYZI_1, config>>();
                preprocess2 = std::make_shared<perception::lidar_preprocess::ConditionalFilter<XYZI_1, XYZI_1, config>>();
                preprocess3 = std::make_shared<perception::lidar_preprocess::RadiusFilter<XYZI_1, XYZI_1, config>>();
                preprocess4 = std::make_shared<perception::lidar_preprocess::VoxelFilter<XYZI_1, XYZI_1, config>>();
                preprocess5 = std::make_shared<perception::lidar_preprocess::AverageGroundSeg<XYZI_1, XYZI_1, average_seg_config>>();
                MultiProcessPoints_ = std::make_shared<perception::tool::MultiProcessPoints>();
                // PatchworkppGroundSeg.reset(new PatchWorkpp<XYZI_1>(&pnh));
                executor = std::make_shared<std::threadpool>(max_thread_num);
                groud_points_all.reset(new pcl::PointCloud<XYZI_1>);
                for (int i = 1; i < max_lidar_num + 1; i++)
                {
                    int num = i + 100;
                    int num2 = i + 1000;
                    pcl::PointCloud<XYZI_1>::Ptr SET_NAME(name, num)(new pcl::PointCloud<XYZI_1>);
                    pcl::PointCloud<XYZI_1>::Ptr SET_NAME(name, num2)(new pcl::PointCloud<XYZI_1>);
                    data_out_all.emplace_back(SET_NAME(name, num));
                    data_in_all.emplace_back(SET_NAME(name, num2));
                }
                private_nh_->declare_parameter<bool>("debug", debug);
                private_nh_->get_parameter("debug", debug);
            }
            ~PreProcess() {}
            int Process(LidarDataInType &data_in, LidarPoint &no_ground_points, LidarPoint &ground_points, config &config_);

        private:
            int PreprocessSingle(pcl::PointCloud<XYZI_1>::Ptr &data_in, pcl::PointCloud<XYZI_1>::Ptr &no_ground_points,
                                 config &config_);
            int PreprocessMulti(LidarDataInType &data_in, LidarPoint &no_ground_points, LidarPoint &ground_points, config &config_);
            int PreprocessPatchWork(LidarDataInType &data_in, LidarPoint &no_ground_points, LidarPoint &ground_points, config &config_);

            std::shared_ptr<perception::lidar_preprocess::LidarPreprocess<XYZI_1, XYZI_1, config>> preprocess1;
            std::shared_ptr<perception::lidar_preprocess::LidarPreprocess<XYZI_1, XYZI_1, config>> preprocess2;
            std::shared_ptr<perception::lidar_preprocess::LidarPreprocess<XYZI_1, XYZI_1, config>> preprocess3;
            std::shared_ptr<perception::lidar_preprocess::LidarPreprocess<XYZI_1, XYZI_1, config>> preprocess4;
            std::shared_ptr<perception::lidar_preprocess::LidarPreprocess<XYZI_1, XYZI_1, average_seg_config>> preprocess5;
            std::shared_ptr<perception::tool::MultiProcessPoints> MultiProcessPoints_;
            std::shared_ptr<std::threadpool> executor;
            // std::shared_ptr<perception::lidar_preprocess::PatchWorkpp<XYZI_1>> PatchworkppGroundSeg;
            std::vector<pcl::PointCloud<XYZI_1>::Ptr> data_in_all;
            std::vector<pcl::PointCloud<XYZI_1>::Ptr> data_out_all;
            pcl::PointCloud<XYZI_1>::Ptr groud_points_all;
            std::map<int, int> lidar_map;
            std::vector<std::future<int>> thread_name;
            const int max_lidar_num = 16;
            const int max_thread_num = 16;
            const std::string name = "lidar_multi";
            const std::string name1 = "mutex";
            bool debug = true;
            rclcpp::Node *private_nh_;
            // debug

            void ResetDataout()
            {
                for (int i = 1; i < max_lidar_num + 1; i++)
                {
                    int num = i + 100;
                    pcl::PointCloud<XYZI_1>::Ptr SET_NAME(name, num)(new pcl::PointCloud<XYZI_1>);
                    data_out_all.emplace_back(SET_NAME(name, num));
                    // data_in_all.emplace_back(SET_NAME(name, num));
                }
            }
        };
        // #define INSTANTIATE_PreProcess(T1, XYZI_1) template class PreProcess<T1, XYZI_1>;
    }
}
#endif